#include "a_star.h"


AstarPlanner::AstarPlanner(){
    __debug_mode = false;
    // 添加八邻域
    __neighbor_points.emplace_back(cv::Point(-1, 0), 10);
    __neighbor_points.emplace_back(cv::Point(1, 0), 10);
    __neighbor_points.emplace_back(cv::Point(0, -1), 10);
    __neighbor_points.emplace_back(cv::Point(0, 1), 10);
    // __neighbor_points.emplace_back(cv::Point(-1, -1), 14);
    // __neighbor_points.emplace_back(cv::Point(-1, 1), 14);
    // __neighbor_points.emplace_back(cv::Point(1, -1), 14);
    // __neighbor_points.emplace_back(cv::Point(1, 1), 14);
}

AstarPlanner::~AstarPlanner(){

}

// 设置调试模式
void AstarPlanner::SetDebugMode(bool mode){
    __debug_mode = mode;
}

// 设置地图
void AstarPlanner::SetMap(cv::Mat map){
    __map_mat = map.clone();
    cout <<"[AstarPlanner::SetMap] rows=" << __map_mat.rows 
        << ", cols=" << __map_mat.cols 
        << endl;
    int max_size = std::max(__map_mat.rows, __map_mat.cols);
    if(__cost_cache_mat.size() != max_size*max_size){
        cout << "update cost cache matrix size = " << max_size*max_size << endl;
        __cost_cache_mat.resize(max_size, max_size);
        for(int x = 0; x < max_size; x++){
            for (int y = 0; y < max_size; y++){
                __cost_cache_mat(x,y) = hypot(x, y) * 10;
            }
        }
    }
}

// 计算到起点代价
float AstarPlanner::CalCostG(cv::Point point, cv::Point start, float D){
    return D;
}

// 计算到终点代价
float AstarPlanner::CalCostH(cv::Point point, cv::Point goal, float D){
    int dx = abs(goal.x - point.x);
    int dy = abs(goal.y - point.y);
    // return hypot(dx, dy) * 10;
    return __cost_cache_mat(dx, dy);
    
}

// 计算点是否有效
bool AstarPlanner::IsValid(cv::Point point){
    if(point.x < 0 || point.y < 0){
        return false;
    }
    if(point.x >= __map_mat.cols || point.y >= __map_mat.rows){
        return false;
    }
    // 是否在障碍物中
    if(__map_mat.at<uchar>(point) < 200){
        return false;
    }
    return true;
}

// 判断两点是否相等
bool AstarPlanner::IsEqual(cv::Point point1, cv::Point point2){
    if(point1.x == point2.x && point1.y == point2.y){
        return true;
    }
    return false;
}

// 规划路径
vector<cv::Point> AstarPlanner::GetPath(cv::Point start, cv::Point goal, 
    vector<cv::Point>& expand_poinds){
    vector<cv::Point> astar_points;
    cout << "[AstarPlanner::GetPath] start=(" 
        << start.x << ", " << start.y << "), goal=("
        << goal.x << ", " << goal.y << "), map rows = "
        << __map_mat.rows << ", cols = " << __map_mat.cols 
        << endl;

    
    // 初始化数据
    std::priority_queue<GP*, vector<GP*>, GPcmp> __open_list;
    std::unordered_map<string, GP*> __closed_list;

    // 起点放入open list
    GP* start_node = new GP(start);
    __open_list.push(start_node);

    // 开始计算
    long debug_count = 0;
    while(true && !__open_list.empty()){
        // 获取优先级最高的点 __open_list
        GP* current_node = __open_list.top(); 

        // 删除当前点
        __open_list.pop();
        if (__closed_list.find(current_node->GetId()) != __closed_list.end())
            continue;
        __closed_list.insert(std::make_pair(current_node->GetId(), current_node));

        if(IsEqual(current_node->GetPoint(), goal)){
            // 找到终点，退出循环
            cout << "[AstarPlanner::GetPath] find goal. open_list size: " << __open_list.size() << endl;
            while(current_node->GetParent() != nullptr){
                astar_points.emplace_back(current_node->GetPoint());
                current_node = current_node->GetParent();
            }
            return astar_points;
        }

        // 遍历临近节点
        for(auto& point_data : __neighbor_points){
            auto new_point = current_node->GetPoint() + point_data.first;
            GP* new_node = new GP(new_point, current_node->GetCostG() + CalCostG(new_point, start, point_data.second), CalCostH(new_point, goal, point_data.second));
            // GP* new_node = new GP(new_point, CalCostG(new_point, start, point_data.second), CalCostH(new_point, goal, point_data.second));
            // 检查点是否已经访问过
            if (__closed_list.find(new_node->GetId()) != __closed_list.end())
                continue;
            // 检查点是否有效
            if(!IsValid(new_point)){
                continue;
            }
            
            // 放入open list
            new_node->SetParent(current_node);
            __open_list.push(new_node);
            expand_poinds.emplace_back(new_node->GetPoint());
        }
    }
    
    return astar_points;
}